//
// Created by rebeater on 2021/1/5.
//

#include "imu_decoder.h"
#include <iostream>
#include <fstream>
#include "nav_io.h"

using namespace std;

int decode_24bit_to_int(const uint8_t buffer[3]) {
    unsigned int temp = 0x00000000u;
    temp += (buffer[0] << 24u);
    temp += (buffer[1] << 16u);
    temp += (buffer[2] << 8u);
    return (*(int *) (&temp)) >> 8u;
}

ImuData Stim300::decode(uint8_t *buffer, uint8_t header) {
    double ts = 1.0 / rate;
    ImuData imu;
    imu.gyro[0] = decode_24bit_to_int(buffer) * 1.0 / (1u << 14u) * _deg;
    imu.gyro[1] = decode_24bit_to_int(buffer + 3) * 1.0 / (1u << 14u) * _deg;
    imu.gyro[2] = decode_24bit_to_int(buffer + 6) * 1.0 / (1u << 14u) * _deg;

    imu.acce[0] = decode_24bit_to_int(buffer + 10) * 1.0 / (1u << 19u) * g;
    imu.acce[1] = decode_24bit_to_int(buffer + 13) * 1.0 / (1u << 19u) * g;
    imu.acce[2] = decode_24bit_to_int(buffer + 16) * 1.0 / (1u << 19u) * g;
    return imu;
}


bool Stim300::decode_file(char *in, char *out) {
    ifstream in_file((char *) in, ios::binary);
    ofstream out_file(out);
    ImuData imu;
    uint8_t buffer[50];
    double ts = 1.0 / rate;
    double gpst = 0;
    char time_buffer[30];
    char header_buffer[512];
    in_file.read(time_buffer, 22);
    time_buffer[22] = 0;
    sprintf(header_buffer, "# file :%s \n"
                           "# time : %s \n"
                           "# rate : %d\n"
                           "# g: %.6f\n"
                           "# coordinate : x->right y->forward z->up\n"
                           "# time/s gx/rad/s gy/rad/s gz/rad/s ax/m*s^{-2} ay/m*s^{-2} az/m*s^{-2}\n",
            in,
            time_buffer + 1,
            rate, g);
    printf("%s", header_buffer);
    out_file << header_buffer;
    while (!in_file.eof() and in_file.good()) {
        in_file.read((char *) buffer, 1);

        if (buffer[0] != RATE_ACCE) {
            continue;
        }

        in_file.read((char *) (buffer), 27);
        if (buffer[9] != 0x00 or buffer[19] != 0x00) {
            printf("error status = %d %d t= %f \n", buffer[9], buffer[19], gpst);
//            continue;
        }
        imu = decode(buffer, RATE_ACCE);
        imu.gpst = gpst;
        out_file << imu;

        gpst += ts;
    }
    cout << "total time " << imu.gpst << endl;
    in_file.close();
    out_file.close();

    return true;
}

Stim300::Stim300(int Rate, double g) : rate(Rate), g(g) {
    header = "";
}

WitMpu9250::WitMpu9250(int rate, double g) : rate(rate), g(g) {

}

bool WitMpu9250::decode_file(char *in, char *out) {
    double ts = 1.0 / rate;
    uint8_t buffer[9];
    ImuData imu;
    ifstream in_file(in, ios::binary);
    ofstream out_file(out);
    char time_buffer[30];
    char header_buffer[512];
    in_file.read(time_buffer, 22);
    time_buffer[22] = 0;
    sprintf(header_buffer, "# file :%s \n"
                           "# time : %s \n"
                           "# rate : %d\n"
                           "# g: %.6f\n"
                           "# coordinate : x->right y->forward z->up\n"
                           "# time/s gx/rad/s gy/rad/s gz/rad/s ax/m*s^{-2} ay/m*s^{-2} az/m*s^{-2}\n",
            in,
            time_buffer + 1,
            rate, g);
    out_file<<header_buffer;
    printf("%s",header_buffer);
    imu.gpst = 0;
    while (!in_file.eof() and in_file.good()) {
        in_file.read((char *) buffer, 1);
        if (buffer[0] == 0x55) {
            in_file.read((char *) buffer, 9);
            if (buffer[0] == 0x51) {
                imu.acce[0] = decode_byte_to_int(buffer + 1) * 1.0 / (1u << 15u) * 16 * g;
                imu.acce[1] = decode_byte_to_int(buffer + 3) * 1.0 / (1u << 15u) * 16 * g;
                imu.acce[2] = decode_byte_to_int(buffer + 5) * 1.0 / (1u << 15u) * 16 * g;
            }
            if (buffer[0] == 0x52) {
                imu.gpst += ts;
                imu.gyro[0] = decode_byte_to_int(buffer + 1) * 1.0 / (1u << 15u) * 2000 * _deg;
                imu.gyro[1] = decode_byte_to_int(buffer + 3) * 1.0 / (1u << 15u) * 2000 * _deg;
                imu.gyro[2] = decode_byte_to_int(buffer + 5) * 1.0 / (1u << 15u) * 2000 * _deg;
                out_file << imu;
            }
        }
    }
    cout<<"total time "<<imu.gpst<<endl;
    in_file.close();
    return true;
}

short WitMpu9250::decode_byte_to_int(const uint8_t *buffer) {
    return *(short *) buffer;
}
